#ifndef CUSTOM_NAV_CONTROLLER_HPP_
#define CUSTOM_NAV_CONTROLLER_HPP_

#include <nav2_core/controller.hpp>

#if defined(ROS_DISTRO_JAZZY)
  #include <nav2_core/controller_exceptions.hpp>
  #include <nav2_core/planner_exceptions.hpp>
#else
  #include <nav2_core/exceptions.hpp>
#endif


#include <rclcpp/rclcpp.hpp>

#include <nav_msgs/msg/path.hpp>
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include <nav2_util/robot_utils.hpp>
#include <nav2_util/geometry_utils.hpp>
#include <nav2_util/node_utils.hpp>

#include <geometry_msgs/msg/twist.hpp>

#include <tf2_ros/buffer.hpp>


namespace custom_nav_controller
{
  class CustNavController : public nav2_core::Controller
  {
  public:
    CustNavController();
    ~CustNavController();
    
    void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
                   std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
                   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
    
    void activate() override;
    void deactivate() override;
    void cleanup() override;
    geometry_msgs::msg::TwistStamped computeVelocityCommands(
        const geometry_msgs::msg::PoseStamped &pose,
        const geometry_msgs::msg::Twist &velocity,
        nav2_core::GoalChecker *goal_checker) override;
    void setPlan(const nav_msgs::msg::Path &path) override;
    void setSpeedLimit(const double &speed_limit, const bool &percentage) override;

        // 获取路径中距离当前点最近的点
    geometry_msgs::msg::PoseStamped 
        getNearestTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);

    // 计算目标点方向和当前位置的角度差
    double calcAngDiff(const geometry_msgs::msg::PoseStamped &current_pose,
                           const geometry_msgs::msg::PoseStamped &target_pose);

  private:
    // 插件名称
    std::string plugin_name_;
    // 节点指针
    rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
    // TF坐标转换指针
    std::shared_ptr<tf2_ros::Buffer> tf_;
    // 代价地图
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
    // 全局路径
    nav_msgs::msg::Path global_path_;

    // 最大线性速度
    double max_lin_speed_;
    // double max_x_lin_speed_;
    // double max_y_lin_speed_;
    // 最大角速度
    double max_ang_speed_;


  };
} // namespace custom_nav_controller

#endif // CUSTOM_NAV_CONTROLLER_HPP_